#!/usr/bin/env python3

import rospy
import sys
import moveit_commander
import geometry_msgs.msg
from math import pi
from tf.transformations import quaternion_from_euler

def main():
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_to_cartesian_pose', anonymous=True)

    robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface()
    move_group = moveit_commander.MoveGroupCommander("robot_arm") # Change to your group name

    # Get current pose for reference
    current_pose = move_group.get_current_pose().pose
    rospy.loginfo(f"Current End-Effector Pose:\n{current_pose}")

    # Set the target Pose in space
    pose_goal = geometry_msgs.msg.Pose()
    pose_goal.position.x = 0.022  # meters
    pose_goal.position.y = -0.0017  # meters
    pose_goal.position.z = 0.45  # meters

    # Convert Euler Angles (roll, pitch, yaw) to Quaternion
    # Example: Tool pointing down (rolled 180 degrees / π radians)
    q = quaternion_from_euler(0, 0, 0) # (roll, pitch, yaw)
    pose_goal.orientation.x = q[0]
    pose_goal.orientation.y = q[1]
    pose_goal.orientation.z = q[2]
    pose_goal.orientation.w = q[3]

    move_group.set_pose_target(pose_goal)

    # Plan and Execute
    rospy.loginfo("Moving to target pose...")
    success = move_group.go(wait=True) # `go` will plan and execute

    if success:
        rospy.loginfo("Awesome! Moved to the desired Cartesian pose!")
    else:
        rospy.logwarn("Failed to reach the target pose.")

    move_group.stop()
    move_group.clear_pose_targets()
    moveit_commander.roscpp_shutdown()

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass